package jia;

import java.awt.Point;
import java.util.ArrayList;
import java.util.List;
import java.util.Set;


import env.WorldModel;

import arch.CowboyArch;
import arch.LocalWorldModel;
import jason.asSemantics.*;
import jason.asSyntax.*;
import jason.environment.grid.Location;

/**
 * Is used when the leader delegates a new target to the agent. If the agent
 * already has a target the revise_target should be used.
 * 
 */
public class delegate_target extends DefaultInternalAction {

	private static final long serialVersionUID = 1L;
	private LocalWorldModel model;
	public static long t0, t1;
	
	@Override
	public Object execute(TransitionSystem ts, Unifier un, Term[] args) throws Exception {



		long start = System.currentTimeMillis();
		//Fetch important values
		model = ((CowboyArch) ts.getUserAgArch()).getModel();
		int agX = (int) ((NumberTerm) args[0]).solve();
		int agY = (int) ((NumberTerm) args[1]).solve();
		Atom currentAgentAtom = (Atom) args[2];
		String currentAgent = currentAgentAtom.getFunctor().equals("self") ? ts.getUserAgArch().getAgName() : currentAgentAtom.getFunctor();
		int agentNr = Integer.parseInt(currentAgent.substring(5, currentAgent.length()));
		Point clustPos = null;
		int connectionRange = 5;
		int maxClusterSize = 10;

		//First, check if a fence should be opened
		
		//If a cow is close to closed fence and the corral, the closest agent should open
		
		List<Point> cows = getCows(model);
		ArrayList<Point> fences = new ArrayList<Point>();
		
		int DIST_TO_FENCE = 5;
		
		//Find fences within DIST_TO_FENCE from a cow
		for(Point cow : cows)	
			if(!model.inCorral(new Location(cow.x, cow.y)))
				for(int i = -DIST_TO_FENCE; i <= DIST_TO_FENCE; i++)
					for(int j = -DIST_TO_FENCE; j <= DIST_TO_FENCE; j++)
						if(cow.x + i >= 0 && cow.x + i < model.getWidth() &&
							cow.y + j >= 0 && cow.y + j < model.getHeight())
							if(model.hasObject(WorldModel.FENCE, cow.x + i, cow.y + j)){
								Point p = new Point(cow.x + i, cow.y + j);
								if(!fences.contains(p))
									fences.add(p);
							}


		//Find switches for the fences close to cows
		List<Point> switches = getSwitches(model);
		List<Point> switchToOpen = new ArrayList<Point>();
		for(Point sw : switches)
			for(Point f : fences)
				if(isConnected(sw, f) && !switchToOpen.contains(sw))
					switchToOpen.add(sw);
		
		//If the agent is close enough, send him to the switch
		int DIST_TO_SWITCH = 15;
		
		for(Point sw : switchToOpen){
			//If switch is close enough and no agent is there yet
			if(Math.abs(agX - sw.x) + Math.abs(agY - sw.y) < DIST_TO_SWITCH){
				if(Math.abs(agX - sw.x) + Math.abs(agY - sw.y) == 1){
					model.setTargetAgentPos(agentNr-1,sw.x - 1, sw.y);
					return un.unifies(args[4], new NumberTermImpl(agX)) &&
					un.unifies(args[5], new NumberTermImpl(agY));
				}
				if(getContent(sw.x + 1, sw.y, model) != WorldModel.AGENT &&
					getContent(sw.x - 1, sw.y, model) != WorldModel.AGENT &&
					getContent(sw.x, sw.y + 1, model) != WorldModel.AGENT &&
					getContent(sw.x, sw.y - 1, model) != WorldModel.AGENT){
						
						if(isClear(sw.x + 1, sw.y, model)){
							if(astar.getShortestPath2(agX, agY, sw.x + 1, sw.y, ts, false).succeded()){
								model.setTargetAgentPos(agentNr-1,sw.x + 1, sw.y);
								long end = System.currentTimeMillis();
								t0 += end-start;
								return un.unifies(args[4], new NumberTermImpl(sw.x + 1)) &&
								un.unifies(args[5], new NumberTermImpl(sw.y));
							}

						}
						if(isClear(sw.x - 1, sw.y, model)){
							if(astar.getShortestPath2(agX, agY, sw.x - 1, sw.y, ts, false).succeded()){
								model.setTargetAgentPos(agentNr-1,sw.x - 1, sw.y);
								long end = System.currentTimeMillis();
								t0 += end-start;
								return un.unifies(args[4], new NumberTermImpl(sw.x - 1)) &&
								un.unifies(args[5], new NumberTermImpl(sw.y));
							}
						}
						if(isClear(sw.x, sw.y - 1, model)){
							if(astar.getShortestPath2(agX, agY, sw.x, sw.y - 1, ts, false).succeded()){
								model.setTargetAgentPos(agentNr-1,sw.x, sw.y - 1);
								long end = System.currentTimeMillis();
								t0 += end-start;
								return un.unifies(args[4], new NumberTermImpl(sw.x)) &&
								un.unifies(args[5], new NumberTermImpl(sw.y - 1));
							}
						}					
						if(isClear(sw.x, sw.y + 1, model)){
							if(astar.getShortestPath2(agX, agY, sw.x, sw.y + 1, ts, false).succeded()){
								model.setTargetAgentPos(agentNr-1,sw.x, sw.y + 1);
								long end = System.currentTimeMillis();
								t0 += end-start;
								return un.unifies(args[4], new NumberTermImpl(sw.x)) &&
								un.unifies(args[5], new NumberTermImpl(sw.y + 1));
							}
						}						
					}				
			}
		}
		
/*
		for(Point sw : switches){
			int bestPos = 0;
			int bestDist = Integer.MAX_VALUE;
			Point bestTarget = new Point(sw.x, sw.y);
			
			for(int i = 0; i < agents.size(); i++){
				Solution sol = astar.getShortestPath2(agents.get(i).x, agents.get(i).x, sw.x + 1, sw.y, ts, false);
				if(sol.succeded()){
					int newDist = sol.getPathLength();
					if(newDist < bestDist){
						bestDist = newDist;
						bestPos = i;
						bestTarget = new Point(sw.x + 1, sw.y);
					}
				}
				sol = astar.getShortestPath2(agents.get(i).x, agents.get(i).x, sw.x - 1, sw.y, ts, false);
				if(sol.succeded()){
					int newDist = sol.getPathLength();
					if(newDist < bestDist){
						bestDist = newDist;
						bestPos = i;
						bestTarget = new Point(sw.x - 1, sw.y);
					}
				}
				sol = astar.getShortestPath2(agents.get(i).x, agents.get(i).x, sw.x, sw.y + 1, ts, false);
				if(sol.succeded()){
					int newDist = sol.getPathLength();
					if(newDist < bestDist){
						bestDist = newDist;
						bestPos = i;
						bestTarget = new Point(sw.x, sw.y + 1);
					}
				}
				sol = astar.getShortestPath2(agents.get(i).x, agents.get(i).x, sw.x, sw.y - 1, ts, false);
				if(sol.succeded()){
					int newDist = sol.getPathLength();
					if(newDist < bestDist){
						bestDist = newDist;
						bestPos = i;
						bestTarget = new Point(sw.x, sw.y - 1);
					}
				}
			}
			//If an agent was chosen
			if(bestDist != Integer.MAX_VALUE){
				Point chosen = agents.remove(bestPos);
				//If the current agent is the chosen one, delegate the target
				if(agX == chosen.x && agY == chosen.y ){
					model.setTargetAgentPos(agentNr-1, bestTarget.x, bestTarget.y);

					return un.unifies(args[4], new NumberTermImpl(bestTarget.x)) &&
					un.unifies(args[5], new NumberTermImpl(bestTarget.y));
				}				
			}
		}
		*/
		
		
		/*
		//First, check if a fence should be opened
		
		//If a cow is close to closed fence and the corral, the closest agent should open
		List<Point> cows = getCows(model);
		ArrayList<Point> switches = new ArrayList<Point>();

		
		for(Point c : cows){

			ArrayList<Node> pathNoFences = astar.getShortestPath(c.x, c.y,
					model.getCorralCenter().x, model.getCorralCenter().y, ts, true);
			
			ArrayList<Node> pathFences = astar.getShortestPath(c.x, c.y,
					model.getCorralCenter().x, model.getCorralCenter().y, ts, false);
			
			//A fence should be opened
			if(pathNoFences.size() < 20 && (pathNoFences.size() < pathFences.size() || pathFences.size() == 0)){

				int i = 0;
				
				//Find node in path with fence
				while(!model.hasObject(WorldModel.FENCE, pathNoFences.get(i).getX(), pathNoFences.get(i).getY())){
					i++;
				}
				
				Node n = pathFences.get(i);
				
				//Horizontal fence
				boolean switchIsWest = true;
				boolean switchIsEast = true;
				boolean switchIsNorth = true;
				boolean switchIsSouth = true;
				boolean switchFound = false;
				
				int d = 0;
				
				while((switchIsWest || switchIsEast || switchIsNorth || switchIsSouth) && !switchFound){
					d++;
					if(switchIsEast){
						if(d + n.getX() < model.getWidth() - 1 && model.hasObject(WorldModel.SWITCH, n.getX() + d, n.getY())){
							switchFound = true;
							switchIsNorth = false;
							switchIsSouth = false;
							switchIsWest = false;
						}
						else if(d + n.getX() >= model.getWidth() && !model.hasObject(WorldModel.FENCE, n.getX() + d, n.getY())){
							switchIsEast = false;
						}
					}
					if(switchIsWest){
						if(n.getX() - d >= 0 && model.hasObject(WorldModel.SWITCH, n.getX() - d, n.getY())){
							switchFound = true;
							switchIsNorth = false;
							switchIsSouth = false;
							switchIsEast = false;
						}
						else if(n.getX() - d < 0 || !model.hasObject(WorldModel.FENCE, n.getX() - d, n.getY())){
							switchIsWest = false;
						}
					}
					if(switchIsNorth){
						if(n.getY() - d >= 0 && model.hasObject(WorldModel.SWITCH, n.getX(), n.getY() - d)){
							switchFound = true;
							switchIsEast = false;
							switchIsSouth = false;
							switchIsWest = false;
						}
						else if(n.getY() - d < 0 || !model.hasObject(WorldModel.FENCE, n.getX(), n.getY() - d)){
							switchIsNorth = false;
						}
					}
					if(switchIsSouth){
						if(d + n.getY() < model.getHeight() - 1 && model.hasObject(WorldModel.SWITCH, n.getX(), n.getY() + d)){
							switchFound = true;
							switchIsNorth = false;
							switchIsEast = false;
							switchIsWest = false;
						}
						else if(d + n.getY() >= model.getHeight() && !model.hasObject(WorldModel.FENCE, n.getX(), n.getY() + d)){
							switchIsSouth = false;
						}
					}					
				}

				if(switchFound){
					if(switchIsNorth) switches.add(new Point(n.getX(), n.getY() - d));
					if(switchIsSouth) switches.add(new Point(n.getX(), n.getY() + d));
					if(switchIsEast) switches.add(new Point(n.getX() + d, n.getY()));
					if(switchIsWest) switches.add(new Point(n.getX() - d, n.getY()));
				}				
			}
		}
		
		if(switches.size() > 0)
			return un.unifies(args[4], new NumberTermImpl(switches.get(0).x)) &&
			un.unifies(args[5], new NumberTermImpl(switches.get(0).y-1));	
		*/
		
		//If no fence should be opened, find the clostest cluster
		
		CowCluster[] clusters = ConnectedComponents.getClusters(ts, cowArray(model), connectionRange, maxClusterSize);		
		CowCluster selCluster = null;
		CowCluster secondCluster = null;


		/* Choose cluster closest to corral such that
		 * the cluster is not in the corral
		 */
		for(CowCluster c : clusters){
			if(selCluster == null)
				selCluster = c;
			else if(secondCluster == null)
				if(	AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, selCluster.x, selCluster.y, ts, true).getPathLength() >
					AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, c.x, c.y, ts, true).getPathLength() &&
					!model.inCorral(new Location(c.x, c.y))
				){
					secondCluster = selCluster.clone();
					selCluster = c;
				}
				else{
					secondCluster = c;
				}
				
			else{
				
				if(	AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, selCluster.x, selCluster.y, ts, true).getPathLength() >
				AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, c.x, c.y, ts, true).getPathLength() &&
					!model.inCorral(new Location(c.x, c.y))
				){
					secondCluster = selCluster.clone();
					selCluster = c;
				}
				else if(AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, secondCluster.x, secondCluster.y, ts, true).getPathLength() >
				AStarSearch.trueShortestPath(model.getCorralCenter().x,
					model.getCorralCenter().y, c.x, c.y, ts, true).getPathLength() &&
					!model.inCorral(new Location(c.x, c.y))){
						secondCluster = c;
				}
			}
		}

		//If there exists a cluster, get its position
		if(selCluster != null){
			int betterCount = 0;
			
			for(Point ag : getAgents(model))
				if(Math.abs(ag.x - selCluster.x) + Math.abs(ag.y - selCluster.y) < 
						Math.abs(agX - selCluster.x) +
						Math.abs(agY - selCluster.y))
					betterCount++;			
		
			//If agent in first group
			if(betterCount < 9)
				clustPos = new Point(selCluster.x, selCluster.y);
			
			else if(secondCluster != null)
				clustPos = new Point(secondCluster.x, secondCluster.y);
			
		}
			
		//If a cluster does not exist, stay where you are
		if(clustPos == null){
			long end = System.currentTimeMillis();
			t0 += end-start;
			return un.unifies(args[4], new NumberTermImpl(agX)) &&
			un.unifies(args[5], new NumberTermImpl(agY));
		}


		//Find shortest path from cluster to corral
		Solution clustToCor = AStarSearch.trueShortestPath(clustPos.x, clustPos.y,
				model.getCorralCenter().x, model.getCorralCenter().y, ts, true);

		String direction = "skip";
		if(clustToCor.succeded())
			direction = clustToCor.getFirstMove();

		//Find potential targets, given the direction of the corral
		Point target = new Point(0,0);
				
		Point[] potentialTargets;
		if(direction == "southwest"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x + 3, clustPos.y - 3);
			potentialTargets[1] = new Point(clustPos.x + 2, clustPos.y - 3);				
			potentialTargets[2] = new Point(clustPos.x + 3, clustPos.y - 2);				
			potentialTargets[3] = new Point(clustPos.x + 2, clustPos.y - 2);
			potentialTargets[4] = new Point(clustPos.x + 2, clustPos.y - 1);
			potentialTargets[5] = new Point(clustPos.x + 1, clustPos.y - 2);
			potentialTargets[6] = new Point(clustPos.x + 1, clustPos.y - 1);
			potentialTargets[7] = new Point(clustPos.x + 0, clustPos.y - 1);
			potentialTargets[8] = new Point(clustPos.x + 0, clustPos.y - 0);
		}
		else if(direction == "southeast"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x - 3, clustPos.y - 3);
			potentialTargets[1] = new Point(clustPos.x - 2, clustPos.y - 3);				
			potentialTargets[2] = new Point(clustPos.x - 3, clustPos.y - 2);				
			potentialTargets[3] = new Point(clustPos.x - 2, clustPos.y - 2);
			potentialTargets[4] = new Point(clustPos.x - 2, clustPos.y - 1);
			potentialTargets[5] = new Point(clustPos.x - 1, clustPos.y - 2);
			potentialTargets[6] = new Point(clustPos.x - 1, clustPos.y - 1);
			potentialTargets[7] = new Point(clustPos.x - 0, clustPos.y - 1);
			potentialTargets[8] = new Point(clustPos.x - 0, clustPos.y - 0);
		}
		else if(direction == "northwest"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x + 3, clustPos.y + 3);
			potentialTargets[1] = new Point(clustPos.x + 2, clustPos.y + 3);				
			potentialTargets[2] = new Point(clustPos.x + 3, clustPos.y + 2);				
			potentialTargets[3] = new Point(clustPos.x + 2, clustPos.y + 2);
			potentialTargets[4] = new Point(clustPos.x + 2, clustPos.y + 1);
			potentialTargets[5] = new Point(clustPos.x + 1, clustPos.y + 2);
			potentialTargets[6] = new Point(clustPos.x + 1, clustPos.y + 1);
			potentialTargets[7] = new Point(clustPos.x + 0, clustPos.y + 1);
			potentialTargets[8] = new Point(clustPos.x + 0, clustPos.y + 0);
		}
		else if(direction == "east"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x - 3, clustPos.y + 0);
			potentialTargets[1] = new Point(clustPos.x - 3, clustPos.y + 1);				
			potentialTargets[2] = new Point(clustPos.x - 3, clustPos.y - 1);				
			potentialTargets[3] = new Point(clustPos.x - 2, clustPos.y + 0);
			potentialTargets[4] = new Point(clustPos.x - 2, clustPos.y + 1);
			potentialTargets[5] = new Point(clustPos.x - 2, clustPos.y - 1);
			potentialTargets[6] = new Point(clustPos.x - 1, clustPos.y + 0);
			potentialTargets[7] = new Point(clustPos.x - 1, clustPos.y + 1);
			potentialTargets[8] = new Point(clustPos.x - 1, clustPos.y - 1);
		}
		else if(direction == "west"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x + 3, clustPos.y + 0);
			potentialTargets[1] = new Point(clustPos.x + 3, clustPos.y + 1);				
			potentialTargets[2] = new Point(clustPos.x + 3, clustPos.y - 1);				
			potentialTargets[3] = new Point(clustPos.x + 2, clustPos.y + 0);
			potentialTargets[4] = new Point(clustPos.x + 2, clustPos.y + 1);
			potentialTargets[5] = new Point(clustPos.x + 2, clustPos.y - 1);
			potentialTargets[6] = new Point(clustPos.x + 1, clustPos.y + 0);
			potentialTargets[7] = new Point(clustPos.x + 1, clustPos.y + 1);
			potentialTargets[8] = new Point(clustPos.x + 1, clustPos.y - 1);
		}
		else if(direction == "north"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x + 0, clustPos.y + 3);
			potentialTargets[1] = new Point(clustPos.x + 1, clustPos.y + 3);				
			potentialTargets[2] = new Point(clustPos.x - 1, clustPos.y + 3);				
			potentialTargets[3] = new Point(clustPos.x + 0, clustPos.y + 2);
			potentialTargets[4] = new Point(clustPos.x + 1, clustPos.y + 2);
			potentialTargets[5] = new Point(clustPos.x - 1, clustPos.y + 2);
			potentialTargets[6] = new Point(clustPos.x + 0, clustPos.y + 1);
			potentialTargets[7] = new Point(clustPos.x + 1, clustPos.y + 1);
			potentialTargets[8] = new Point(clustPos.x - 1, clustPos.y + 1);
		}
		else if(direction == "south"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x + 0, clustPos.y - 3);
			potentialTargets[1] = new Point(clustPos.x + 1, clustPos.y - 3);				
			potentialTargets[2] = new Point(clustPos.x - 1, clustPos.y - 3);				
			potentialTargets[3] = new Point(clustPos.x + 0, clustPos.y - 2);
			potentialTargets[4] = new Point(clustPos.x + 1, clustPos.y - 2);
			potentialTargets[5] = new Point(clustPos.x - 1, clustPos.y - 2);
			potentialTargets[6] = new Point(clustPos.x + 0, clustPos.y - 1);
			potentialTargets[7] = new Point(clustPos.x + 1, clustPos.y - 1);
			potentialTargets[8] = new Point(clustPos.x - 1, clustPos.y - 1);
		}
		else if(direction == "northeast"){
			potentialTargets = new Point[9];
			potentialTargets[0] = new Point(clustPos.x - 3, clustPos.y + 3);
			potentialTargets[1] = new Point(clustPos.x - 2, clustPos.y + 3);				
			potentialTargets[2] = new Point(clustPos.x - 3, clustPos.y + 2);				
			potentialTargets[3] = new Point(clustPos.x - 2, clustPos.y + 2);
			potentialTargets[4] = new Point(clustPos.x - 2, clustPos.y + 1);
			potentialTargets[5] = new Point(clustPos.x - 1, clustPos.y + 2);
			potentialTargets[6] = new Point(clustPos.x - 1, clustPos.y + 1);
			potentialTargets[7] = new Point(clustPos.x - 0, clustPos.y + 1);
			potentialTargets[8] = new Point(clustPos.x - 0, clustPos.y + 0);
		}
		else potentialTargets = new Point[0];
		
		//Select most attractive target that is available
		boolean targetSelected = false;
		for(int i = 0; i < potentialTargets.length; i++){
			if(isClear(potentialTargets[i].x, potentialTargets[i].y, model)){
				targetSelected = true;
				target = new Point(potentialTargets[i].x, potentialTargets[i].y);
				break;
			}
		}
		if(!targetSelected)
			target = new Point(clustPos.x, clustPos.y);
			
//		if(selCluster != null)
//			System.out.println("Primary: " + selCluster.x + "," + selCluster.y);
//		if(secondCluster != null)
//			System.out.println("Secondary: " + secondCluster.x + "," + secondCluster.y);
//		
//		System.out.println(target);
//		System.out.println("direction: " + direction);
//		System.out.println("-----------------");
		
		
		//Make sure the target is on the map
		if(target.x < 0) target.x = 0;
		if(target.x >= model.getWidth()) target.x = model.getWidth() - 1;
		if(target.y < 0) target.y = 0;
		if(target.y >= model.getHeight()) target.y = model.getHeight() - 1;
		
		if(isClear(target.x, target.y, model)){
			model.setTargetAgentPos(agentNr-1, target.x, target.y);
			
			long end = System.currentTimeMillis();
			t0 += end-start;
			return un.unifies(args[4], new NumberTermImpl(target.x)) &&
			un.unifies(args[5], new NumberTermImpl(target.y));
		}
		else{
			model.setTargetAgentPos(agentNr-1, clustPos.x, clustPos.y);
			
			long end = System.currentTimeMillis();
			t0 += end-start;
			return un.unifies(args[4], new NumberTermImpl(clustPos.x)) &&
			un.unifies(args[5], new NumberTermImpl(clustPos.y));
		}
			
			
	}
	
    private int getContent(int x, int y, LocalWorldModel model) {
    	if (model.hasObject(WorldModel.CLEAN, x, y))
    		return WorldModel.CLEAN;
    	else if (model.hasObject(WorldModel.AGENT, x, y))
    		return WorldModel.AGENT;
    	else if (model.hasObject(WorldModel.OBSTACLE, x, y))
    		return WorldModel.OBSTACLE;
    	else if (model.hasObject(WorldModel.COW, x, y))
    		return WorldModel.COW;
    	else if (model.hasObject(WorldModel.CORRAL, x, y))
    		return WorldModel.CORRAL;
    	else if (model.hasObject(WorldModel.ENEMY, x, y))
    		return WorldModel.ENEMY;
    	else if (model.hasObject(WorldModel.ENEMYCORRAL, x, y))
    		return WorldModel.ENEMYCORRAL;
    	else if (model.hasObject(WorldModel.FENCE, x, y))
    		return WorldModel.FENCE;
    	else if (model.hasObject(WorldModel.SWITCH, x, y))
    		return WorldModel.SWITCH;
    	
    	return 0;
    }
    
    private boolean isClear(int x, int y, LocalWorldModel model) {
    	int content = getContent(x,y,model);
    	return content != WorldModel.OBSTACLE &&
    		content != WorldModel.FENCE &&
    		content != WorldModel.SWITCH;
    }
	
	//Get the cow positions of the map
	private int[][] cowArray(LocalWorldModel model) {
		int[][] cowsInMap = new int[model.getWidth()][model.getHeight()];
		for (int i = 0; i < model.getWidth(); i++) {
			for (int j = 0; j < model.getHeight(); j++) {
				if (model.hasObject(WorldModel.COW, i, j))
					cowsInMap[i][j] = 1;
			}
		}
		return cowsInMap;
	}
	
	//Get the cow positions
	private List<Point> getCows(LocalWorldModel model) {
		ArrayList<Point> cows = new ArrayList<Point>();
		for (int i = 0; i < model.getWidth(); i++) {
			for (int j = 0; j < model.getHeight(); j++) {
				if (model.hasObject(WorldModel.COW, i, j))
					cows.add(new Point(i,j));
			}
		}
		return cows;
	}
	
	//Get positions of the switches
	private List<Point> getSwitches(LocalWorldModel model){
		ArrayList<Point> switches = new ArrayList<Point>();
		for (int i = 0; i < model.getWidth(); i++) {
			for (int j = 0; j < model.getHeight(); j++) {
				if (model.hasObject(WorldModel.SWITCH, i, j))
					switches.add(new Point(i,j));
			}
		}
		return switches;
	}
		
	//Get positions of the agents
	private ArrayList<Point> getAgents(LocalWorldModel model){
		ArrayList<Point> agents = new ArrayList<Point>();
		for (int i = 0; i < 20; i++) {
			if(i != 2 && i != 3 && i != 4){
				Location loc = model.getAgPos(i);
				agents.add(new Point(loc.x, loc.y));
			}
		}
		return agents;
	}
	
	//Returns true if the fence is connected to the switch
	private boolean isConnected(Point aSwitch, Point aFence){
		//Switch and fence must be on same horizontal or vertical line
		if(aSwitch.x != aFence.x && aSwitch.y != aFence.y)
			return false;
		
		//Switch is east of the fence
		if(aSwitch.x > aFence.x){
			for(int i = aFence.x + 1; i < aSwitch.x; i++)
				if (!model.hasObject(WorldModel.FENCE, i, aSwitch.y))
					return false;
			return true;
		}
		
		//Switch is west of the fence
		if(aSwitch.x < aFence.x){
			for(int i = aSwitch.x + 1; i < aFence.x; i++)
				if (!model.hasObject(WorldModel.FENCE, i, aSwitch.y))
					return false;
			return true;
		}
		
		//Switch is north of the fence
		if(aSwitch.y < aFence.y){
			for(int i = aSwitch.y + 1; i < aFence.y; i++)
				if (!model.hasObject(WorldModel.FENCE, aSwitch.x, i))
					return false;
			return true;
		}
		
		//Switch is north of the fence
		if(aSwitch.y > aFence.y){
			for(int i = aFence.y + 1; i < aSwitch.y; i++)
				if (!model.hasObject(WorldModel.FENCE, aSwitch.x, i))
					return false;
			return true;
		}
		
		//Unreachable
		return false;
	}
}

class Target {
	public Target(Point target) {
		this.target = target;
	}
	
	public Target() {
		this.target = new Point();
	}
	
	Point target;
	int agent = -1;

	public String toString() {
		return "Agent" + agent + " -> " + target;
	}
}